﻿#pragma once

#include <initializer_list>
#include <iostream>
#include <random>
#include <vector>

#include <xy/math/math.h>

namespace xy
{

namespace math
{

template <typename Real> class matx
{
  protected:
    std::size_t m_row, m_col;
    std::vector<Real> m_data;

  public:
    matx(const matx &rhs) = default;
    matx(matx &&rhs) = default;
    ~matx() = default;

    /**
     * @brief 特殊构造
     *
     * @param[in] t
     */
    matx(Real t) : m_row(1), m_col(1), m_data(1, t)
    {
    }

    /**
     * @brief 标准构造
     *
     * @param[in] row
     * @param[in] col
     */
    explicit matx(std::size_t row, std::size_t col) : m_row(row), m_col(col), m_data(row * col)
    {
    }

    /**
     * @brief 赋值
     *
     * @param[in] rhs
     * @return matx&
     */
    matx &operator=(const matx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
            m_data = rhs.m_data;
        return *this;
    }

    /**
     * @brief 移动
     *
     * @param[in] rhs
     * @return matx&
     */
    matx &operator=(matx &&rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
            m_data = std::move(rhs.m_data);
        return *this;
    }

    /**
     * @brief 重置大小
     *
     * @param[in] row
     * @param[in] col
     * @param[in] keep
     */
    void resize(std::size_t row, std::size_t col, bool keep = false)
    {
        if (keep)
        {
            std::vector<Real> tmp(row * col);
            std::size_t m1 = std::min(row, m_row);
            std::size_t m2 = std::min(col, m_col);
            for (std::size_t i = 0; i < m1; ++i)
                for (std::size_t j = 0; j < m2; ++j)
                    tmp[i * col + j] = m_data[i * m_col + j];
            m_data = std::move(tmp);
        }
        else
            m_data.resize(row * col);

        m_row = row;
        m_col = col;
    }

    /**
     * @brief 初始化为零
     *
     * @return matx&
     */
    matx &set_zero()
    {
        std::fill(m_data.begin(), m_data.end(), 0);
        return *this;
    }

    /**
     * @brief 初始化为 1
     *
     * @return matx&
     */
    matx &set_one()
    {
        std::fill(m_data.begin(), m_data.end(), 1);
        return *this;
    }

    /**
     * @brief 填满值
     *
     * @param[in] value
     * @return matx&
     */
    matx &set_value(Real value)
    {
        std::fill(m_data.begin(), m_data.end(), value);
        return *this;
    }

    /**
     * @brief 填充随机值
     *
     * @param[in] min
     * @param[in] max
     * @return matx&
     */
    matx &set_random(Real min = 0, Real max = 1)
    {
        std::random_device rd{};
        std::mt19937 gen(rd());
        std::uniform_real_distribution<Real> dis(min, max);
        for (auto &x : m_data)
            x = dis(gen);
        return *this;
    }

    /**
     * @brief 获得行数
     *
     * @return std::size_t
     */
    std::size_t rows() const noexcept
    {
        return m_row;
    }

    /**
     * @brief 获得列数
     *
     * @return std::size_t
     */
    std::size_t cols() const noexcept
    {
        return m_col;
    }

    /**
     * @brief 获得数据数组
     *
     * @return const Real*
     */
    const Real *data() const noexcept
    {
        return m_data.data();
    }

    /**
     * @brief 获得数据数组
     *
     * @return Real*
     */
    Real *data() noexcept
    {
        return m_data.data();
    }

    /**
     * @brief 展平为向量，允许退化为 1 维数组访问
     *
     * @return auto
     */
    const auto &flatten() const noexcept
    {
        return m_data;
    }

    auto &flatten() noexcept
    {
        return m_data;
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return Real
     */
    Real operator()(std::size_t i, std::size_t j) const
    {
        if (i >= m_row || j >= m_col)
            throw std::out_of_range("Index out of range");
        return m_data[i * m_col + j];
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return Real&
     */
    Real &operator()(std::size_t i, std::size_t j)
    {
        if (i >= m_row || j >= m_col)
            throw std::out_of_range("Index out of range");
        return m_data[i * m_col + j];
    }

    /**
     * @brief 矩阵加法
     *
     * @param[in] rhs
     * @return matx
     */
    matx operator+(const matx &rhs) const
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        matx res(m_row, m_col);
        std::transform(m_data.begin(), m_data.end(), rhs.m_data.begin(), res.m_data.begin(), std::plus<Real>());
        return res;
    }

    /**
     * @brief 矩阵加法
     *
     * @param[in] rhs
     * @return matx&
     */
    const matx &operator+=(const matx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        std::transform(m_data.begin(), m_data.end(), rhs.m_data.begin(), m_data.begin(), std::plus<Real>());
        return *this;
    }

    /**
     * @brief 矩阵减法
     *
     * @param[in] rhs
     * @return matx
     */
    matx operator-(const matx &rhs) const
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        matx res(m_row, m_col);
        std::transform(m_data.begin(), m_data.end(), rhs.m_data.begin(), res.m_data.begin(), std::minus<Real>());
        return res;
    }

    /**
     * @brief 矩阵减法
     *
     * @param[in] rhs
     * @return matx&
     */
    const matx &operator-=(const matx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        std::transform(m_data.begin(), m_data.end(), rhs.m_data.begin(), m_data.begin(), std::minus<Real>());
        return *this;
    }

    /**
     * @brief 倍乘
     *
     * @param[in] t
     * @return matx
     */
    matx operator*(Real t) const
    {
        matx res(m_row, m_col);
        std::transform(m_data.begin(), m_data.end(), res.m_data.begin(), [t](Real x) { return x * t; });
        return res;
    }

    /**
     * @brief 倍乘
     *
     * @param[in] t
     * @return matx&
     */
    matx &operator*=(Real t)
    {
        std::transform(m_data.begin(), m_data.end(), m_data.begin(), [t](Real x) { return x * t; });
        return *this;
    }

    /**
     * @brief 除法
     *
     * @param[in] t
     * @return matx
     */
    matx operator/(Real t) const
    {
        if (std::abs(t) < std::numeric_limits<Real>::epsilon())
            throw std::runtime_error("Divide by zero");
        matx res(m_row, m_col);
        std::transform(m_data.begin(), m_data.end(), res.m_data.begin(), [t](Real x) { return x / t; });
        return res;
    }

    /**
     * @brief 除法
     *
     * @param[in] t
     * @return matx&
     */
    const matx &operator/=(Real t)
    {
        if (std::abs(t) < std::numeric_limits<Real>::epsilon())
            throw std::runtime_error("Divide by zero");
        std::transform(m_data.begin(), m_data.end(), m_data.begin(), [t](Real x) { return x / t; });
        return *this;
    }

    /**
     * @brief 比较
     *
     * @param[in] rhs
     * @return true
     * @return false
     */
    bool operator==(const matx &rhs) const
    {
        return (rhs - *this).norm() < std::numeric_limits<Real>::epsilon();
    }

    /**
     * @brief 取反
     *
     * @return matx
     */
    matx operator-() const
    {
        matx res(m_row, m_col);
        std::generate(res.m_data.begin(), res.m_data.end(), [it = m_data.begin()]() mutable { return -*it++; });
        return res;
    }

    /**
     * @brief 矩阵乘法
     *
     * @param[in] rhs
     * @return matx
     */
    matx operator*(const matx &rhs) const
    {
        if (m_col != rhs.m_row)
            throw std::logic_error("matx size not match");
        matx res(m_row, rhs.m_col);
        for (std::size_t i = 0; i < m_row; ++i)
            for (std::size_t j = 0; j < rhs.m_col; ++j)
                for (std::size_t k = 0; k < m_col; ++k)
                    res(i, j) += m_data[i * m_col + k] * rhs(k, j);
        return res;
    }

    /**
     * @brief Kronecker 积
     *
     * @param[in] rhs
     * @return matx
     */
    matx operator%(const matx &rhs) const
    {
        matx res(m_row * rhs.m_row, m_col * rhs.m_col);
        for (std::size_t i0 = 0; i0 < m_row; i0++)
            for (std::size_t j0 = 0; j0 < m_col; j0++)
                for (std::size_t i1 = 0; i1 < rhs.m_row; i1++)
                    for (std::size_t j1 = 0; j1 < rhs.m_col; j1++)
                        res(i0 * rhs.m_row + i1, j0 * rhs.m_col + j1) = m_data[i0 * m_col + j0] * rhs(i1, j1);
        return res;
    }

    /**
     * @brief 在指定位置填充矩阵
     *
     * @param[in] i
     * @param[in] j
     * @param[in] rhs
     * @return matx&
     */
    matx &fill(std::size_t i, std::size_t j, const matx &rhs)
    {
        if (i + rhs.rows() > m_row || j + rhs.cols() > m_col)
            throw std::out_of_range("Index out of range");
        for (std::size_t ii = 0; ii < rhs.rows(); ++ii)
            for (std::size_t jj = 0; jj < rhs.cols(); ++jj)
                m_data[(i + ii) * m_col + (j + jj)] = rhs(ii, jj);
        return *this;
    }

    /**
     * @brief 范数
     *
     * @param[in] nt
     * @return Real
     */
    Real norm(int nt = 0) const
    {
        switch (nt)
        {
        case 0: // inf-norm
        {
            Real r = 0;
            for (std::size_t i = 0; i < m_row; i++)
            {
                Real a = 0;
                for (std::size_t j = 0; j < m_col; j++)
                    a += std::abs(m_data[i * m_col + j]);
                r = std::max(a, r);
            }
            return r;
        }
        case 1: // 1-norm
        {
            Real r = 0;
            for (std::size_t i = 0; i < m_col; i++)
            {
                Real a = 0;
                for (std::size_t j = 0; j < m_row; j++)
                    a += std::abs(m_data[j * m_col + i]);
                r = std::max(a, r);
            }
            return r;
        }
        case 2: // f-norm
            // 注意这里 Real{} 将决定返回值类型，如果直接设为 0 会按照 int 类型返回，导致结果错误
            return std::inner_product(m_data.begin(), m_data.end(), m_data.begin(), Real{});
        default:
            throw std::logic_error("Invalid norm type");
            return 0;
        }
    }

    /**
     * @brief 迹
     *
     * @return Real
     */
    Real trace() const
    {
        Real tr = 0;
        std::size_t n = std::min(m_row, m_col);
        for (std::size_t i = 0; i < n; ++i)
            tr += m_data[i * m_col + i];
        return tr;
    }

    /**
     * @brief 转置矩阵
     *
     * @return matx&
     */
    matx &transpose()
    {
        std::vector<Real> tmp(m_row * m_col);
        for (std::size_t i = 0; i < m_row; ++i)
            for (std::size_t j = 0; j < m_col; ++j)
                tmp[j * m_row + i] = m_data[i * m_col + j];
        m_data = std::move(tmp);
        std::swap(m_row, m_col);
        return *this;
    }

    /**
     * @brief 转置
     *
     * @return matx
     */
    matx transposed() const
    {
        matx res(m_col, m_row);
        for (std::size_t i = 0; i < m_row; ++i)
            for (std::size_t j = 0; j < m_col; ++j)
                res(j, i) = m_data[i * m_col + j];
        return res;
    }

    /**
     * @brief 单位阵
     *
     * @param[in] n
     * @return matx
     */
    static matx identity(std::size_t n)
    {
        matx res(n, n);
        res.set_zero();
        for (std::size_t i = 0; i < n; ++i)
            res(i, i) = 1;
        return res;
    }

    /**
     * @brief 行交换矩阵
     *
     * @param[in] n
     * @param[in] i
     * @param[in] j
     * @return matx
     */
    static matx exchange(std::size_t n, std::size_t i, std::size_t j)
    {
        matx res = identity(n);
        res(i, i) = res(j, j) = 0;
        res(i, j) = res(j, i) = 1;
        return res;
    }

    /**
     * @brief 行乘矩阵
     *
     * @param[in] n
     * @param[in] i
     * @param[in] t
     * @return matx
     */
    static matx times(std::size_t n, std::size_t i, Real t)
    {
        matx res = identity(n);
        res(i, i) = t;
        return res;
    }

    /**
     * @brief 行加乘矩阵
     *
     * @param[in] n
     * @param[in] i
     * @param[in] j
     * @param[in] t
     * @return matx
     */
    static matx times_add(std::size_t n, std::size_t i, std::size_t j, Real t)
    {
        matx res = identity(n);
        res(i, j) = t;
        return res;
    }

    /**
     * @brief 严格对角线占优矩阵（必然非奇异）
     *
     * @param[in] n
     * @return matx
     */
    static matx strictly_diagonally_dominant(std::size_t n)
    {
        std::mt19937 rng(std::random_device{}());
        std::uniform_real_distribution<Real> dist(-1, 1);

        matx res(n, n);
        for (std::size_t i = 0; i < n; ++i)
        {
            Real sum = 0;
            for (std::size_t j = 0; j < i; ++j)
            {
                res(i, j) = dist(rng);
                sum += std::abs(res(i, j));
            }

            for (std::size_t j = i + 1; j < n; ++j)
            {
                res(i, j) = dist(rng);
                sum += std::abs(res(i, j));
            }

            // 这里对角元为正，其实也可以为负
            res(i, i) = sum + std::abs(dist(rng));
        }
        return res;
    }

    /**
     * @brief 对称正定矩阵
     *
     * @param[in] n
     * @return matx
     */
    static matx symmetric_positive_definite(std::size_t n)
    {
        std::mt19937 rng(std::random_device{}());
        std::uniform_real_distribution<Real> dist(-1, 1);

        matx res(n, n);
        for (std::size_t i = 0; i < n; ++i)
        {
            Real sum = 0;
            for (std::size_t j = 0; j < i; ++j)
            {
                res(i, j) = res(j, i);
                sum += std::abs(res(i, j));
            }

            for (std::size_t j = i; j < n; ++j)
            {
                res(i, j) = dist(rng);
                sum += std::abs(res(i, j));
            }

            // 对称严格对角占优矩阵，如果对角元为正，则正定
            res(i, i) = sum + std::abs(dist(rng));
        }
        return res;
    }
};

template <typename Real> class vecx : public matx<Real>
{
    // 引入基类成员
    using matx<Real>::m_row;
    using matx<Real>::m_col;
    using matx<Real>::m_data;

  public:
    vecx(const vecx &rhs) = default;
    vecx(vecx &&rhs) = default;

    /**
     * @brief 向量构造函数
     *
     * @param[in] n
     */
    explicit vecx(std::size_t n) : matx<Real>(n, 1)
    {
    }

    /**
     * @brief 初始化列表构造
     *
     * @param[in] list
     */
    vecx(std::initializer_list<Real> list) : matx<Real>(list.size(), 1)
    {
        std::copy(list.begin(), list.end(), m_data.begin());
    }

    /**
     * @brief 拷贝构造
     *
     * @param[in] rhs
     */
    vecx(const matx<Real> &rhs) : matx<Real>(rhs)
    {
        if (rhs.cols() != 1 && rhs.rows() != 1)
            throw std::logic_error("Invalid matrix size");
    }

    /**
     * @brief 移动构造
     *
     * @param[in] rhs
     */
    vecx(matx<Real> &&rhs) : matx<Real>(std::move(rhs))
    {
        if (rhs.cols() != 1 && rhs.rows() != 1)
            throw std::logic_error("Invalid matrix size");
    }

    /**
     * @brief 拷贝赋值
     *
     * @param[in] rhs
     * @return vecx&
     */
    vecx &operator=(const vecx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
            m_data = rhs.m_data;
        return *this;
    }

    /**
     * @brief 移动赋值
     *
     * @param[in] rhs
     * @return vecx&
     */
    vecx &operator=(vecx &&rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
            m_data = std::move(rhs.m_data);
        return *this;
    }

    /**
     * @brief 索引
     *
     * @param[in] i
     * @return Real
     */
    Real operator[](std::size_t i) const
    {
        if (i >= size())
            throw std::out_of_range("Index out of range");
        return m_data[i];
    }

    /**
     * @brief 索引
     *
     * @param[in] i
     * @return Real&
     */
    Real &operator[](std::size_t i)
    {
        if (i >= size())
            throw std::out_of_range("Index out of range");
        return m_data[i];
    }

    /**
     * @brief 向量长度
     *
     * @return std::size_t
     */
    std::size_t size() const noexcept
    {
        return m_data.size();
    }
};

template <typename Real> struct spvecx
{
    // 稀疏矩阵向量允许元素索引是负数
    std::vector<std::tuple<int, int, Real>> data;
    int m{0}, n{0}; // 记录矩阵大小

    /**
     * @brief 构建元素
     *
     * @param[in] x
     * @param[in] y
     * @param[in] value
     */
    void create(int x, int y, Real value)
    {
        m = std::max(m, x + 1);
        n = std::max(n, y + 1);
        data.emplace_back(x, y, value);
    }

    /**
     * @brief 将 spvecx 视为向量，计算乘法 lhs * rhs^T
     *
     * @param[in] rhs
     * @return spvecx
     */
    const spvecx operator%(const spvecx &rhs)
    {
        spvecx res;
        for (auto &[x, y, value] : data)
            for (auto &[u, v, w] : rhs.data)
                res.create(x * n + y, u * rhs.n + v, value * w);
        return res;
    }

    /**
     * @brief 遍历元素
     *
     * @tparam Func
     * @param[in] func
     */
    template <class Func> void foreach (Func &&func)
    {
        for (auto &[x, y, value] : data)
            func(x, y, value);
    }

    /**
     * @brief 转换为矩阵
     *
     * @return matx<Real>
     */
    matx<Real> to_matrix() const
    {
        matx<Real> res(m, n);
        res.set_zero();
        for (auto &[x, y, value] : data)
            res(x, y) = value;
        return res;
    }
};

template <typename Real> class spmatx
{
  protected:
    std::size_t m_col; /// 列数
    std::size_t m_row; /// 行数

    std::vector<std::size_t> m_IA; /// 每个元素记录每行开始位置的全局索引，最后一个元素记录有效元个数
    std::vector<std::size_t> m_JA; /// 每个元素记录列标
    std::vector<Real> m_AA;        /// 每个元素记录矩阵元素

  public:
    spmatx(const spmatx &rhs) = default;
    spmatx(spmatx &&rhs) = default;
    ~spmatx() = default;

    /**
     * @brief 空构造
     *
     * @param[in] row
     * @param[in] col
     */
    explicit spmatx(std::size_t row, std::size_t col) : m_row(row), m_col(col), m_IA(row + 1, 0)
    {
        // 只需要构造边界值
        m_JA.push_back(std::numeric_limits<std::size_t>::infinity());
        m_AA.push_back(std::numeric_limits<Real>::infinity());
    }

    /**
     * @brief 拷贝赋值
     *
     * @param[in] rhs
     * @return spmatx&
     */
    spmatx &operator=(const spmatx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
        {
            m_IA = rhs.m_IA;
            m_JA = rhs.m_JA;
            m_AA = rhs.m_AA;
        }
        return *this;
    }

    /**
     * @brief 移动赋值
     *
     * @param[in] rhs
     * @return spmatx&
     */
    spmatx &operator=(spmatx &&rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        if (this != &rhs) [[likely]]
        {
            m_IA = std::move(rhs.m_IA);
            m_JA = std::move(rhs.m_JA);
            m_AA = std::move(rhs.m_AA);
        }
        return *this;
    }

    /**
     * @brief 隐式转换为矩阵
     *
     * @return matx<Real>
     */
    operator matx<Real>() const
    {
        matx<Real> res(m_row, m_col);
        res.set_zero();
        for (std::size_t i = 0; i < m_row; i++)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                res(i, m_JA[j]) = m_AA[j];
        }
        return res;
    }

    /**
     * @brief 重置大小
     *
     * @param[in] row
     * @param[in] col
     */
    void resize(std::size_t row, std::size_t col)
    {
        m_row = row;
        m_col = col;
        m_IA.clear();
        m_IA.resize(row + 1, 0); // 注意只有新增的元素被初始化为 0
        m_JA.clear();
        m_AA.clear();
        m_JA.push_back(std::numeric_limits<std::size_t>::infinity());
        m_AA.push_back(std::numeric_limits<Real>::infinity());
    }

    /**
     * @brief 获得行数
     *
     * @return std::size_t
     */
    std::size_t rows() const noexcept
    {
        return m_row;
    }

    /**
     * @brief 获得列数
     *
     * @return std::size_t
     */
    std::size_t cols() const noexcept
    {
        return m_col;
    }

    /**
     * @brief 获得非零元素个数
     *
     * @return std::size_t
     */
    std::size_t nnz() const noexcept
    {
        return m_IA.back();
    }

    /**
     * @brief 获得存在的元素，不存在的元素认为是零
     *
     * @param[in] i
     * @param[in] j
     * @return Real
     */
    Real operator()(std::size_t i, std::size_t j) const
    {
        if (i >= m_row || j >= m_col)
            throw std::out_of_range("Index out of range");
        for (std::size_t k = m_IA[i]; k < m_IA[i + 1]; k++)
            if (m_JA[k] == j)
                return m_AA[k];
        return Real{};
    }

    /**
     * @brief 获得存在的元素，不存在则创建元素
     *
     * @param[in] i
     * @param[in] j
     * @return Real&
     */
    Real &operator()(std::size_t i, std::size_t j)
    {
        if (i >= m_row || j >= m_col)
            throw std::out_of_range("Index out of range");

        // 如果找到了就直接设置，然后立即返回
        for (std::size_t k = m_IA[i]; k < m_IA[i + 1]; k++)
            if (m_JA[k] == j)
                return m_AA[k];

        // 否则，就插入新元素；找到第一个元素，其列标大于 j，则它是插入位置
        std::size_t k = m_IA[i];
        for (; k < m_IA[i + 1]; k++)
        {
            if (m_JA[k] > j)
                break;
        }

        // 插入列标和元素
        Real value{};
        auto it_J = m_JA.begin() + k;
        auto it_A = m_AA.begin() + k;
        it_J = m_JA.insert(it_J, j);
        it_A = m_AA.insert(it_A, value);

        // 将 i 行后记录的索引都加 1，注意也包括最后一个元素
        for (std::size_t k = i + 1; k < m_row + 1; k++)
            m_IA[k]++;
        return *it_A;
    }

    /**
     * @brief 加法
     *
     * @param[in] rhs
     * @return spmatx
     */
    spmatx operator+(const spmatx &rhs) const
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        spmatx res = *this;
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = rhs.m_IA[i]; j < rhs.m_IA[i + 1]; j++)
                res(i, rhs.m_JA[j]) += rhs.m_AA[j];
        }
        return res;
    }

    /**
     * @brief 加法
     *
     * @param[in] rhs
     * @return spmatx&
     */
    const spmatx &operator+=(const spmatx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = rhs.m_IA[i]; j < rhs.m_IA[i + 1]; j++)
                operator()(i, rhs.m_JA[j]) += rhs.m_AA[j];
        }
        return *this;
    }

    /**
     * @brief 减法
     *
     * @param[in] rhs
     * @return spmatx
     */
    spmatx operator-(const spmatx &rhs) const
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        spmatx res = *this;
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = rhs.m_IA[i]; j < rhs.m_IA[i + 1]; j++)
                res(i, rhs.m_JA[j]) -= rhs.m_AA[j];
        }
        return res;
    }

    /**
     * @brief 减法
     *
     * @param[in] rhs
     * @return spmatx&
     */
    const spmatx &operator-=(const spmatx &rhs)
    {
        if (m_row != rhs.m_row || m_col != rhs.m_col)
            throw std::logic_error("matx size not match");
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = rhs.m_IA[i]; j < rhs.m_IA[i + 1]; j++)
                operator()(i, rhs.m_JA[j]) -= rhs.m_AA[j];
        }
        return *this;
    }

    /**
     * @brief 乘法
     *
     * @param[in] k
     * @return spmatx
     */
    spmatx operator*(Real k) const
    {
        spmatx res = *this;
        for (std::size_t i = 0; i < m_AA.size() - 1; i++)
            res.m_AA[i] *= k;
        return res;
    }

    /**
     * @brief 乘法
     *
     * @param[in] k
     * @return spmatx&
     */
    spmatx &operator*=(Real k)
    {
        for (std::size_t i = 0; i < m_AA.size() - 1; i++)
            m_AA[i] *= k;
        return *this;
    }

    /**
     * @brief 除法
     *
     * @param[in] k
     * @return spmatx
     */
    spmatx operator/(Real k) const
    {
        if (std::abs(k) < std::numeric_limits<Real>::epsilon())
            throw std::logic_error("Divide by zero");
        return *this * (1 / k);
    }

    /**
     * @brief 除法
     *
     * @param[in] k
     * @return spmatx&
     */
    const spmatx &operator/=(Real k)
    {
        if (std::abs(k) < std::numeric_limits<Real>::epsilon())
            throw std::logic_error("Divide by zero");
        return *this *= (1 / k);
    }

    /**
     * @brief 稀疏矩阵乘稀疏矩阵
     *
     * @param[in] rhs
     * @return spmatx
     */
    spmatx operator*(const spmatx &rhs) const
    {
        if (m_col != rhs.m_row)
            throw std::logic_error("matx size not match");

        // 暂存数据
        std::size_t col2 = rhs.m_col;
        static std::vector<Real> tmp(col2);

        spmatx res(m_row, rhs.m_col);
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // 初始化为 0
            std::fill(tmp.begin(), tmp.end(), 0);

            // 计算 i 行的结果
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
            {
                // 获得对应 JA[IA[i]] 列的 rhs 的 JA[IA[i]] 行元素中非零列标索引
                std::size_t start = rhs.m_IA[m_JA[j]];
                std::size_t end = rhs.m_IA[m_JA[j] + 1];

                // 这是一个很巧妙的计算方式，只需要每次计算两个元素的乘积，记录在对应的列的位置上
                // 然后不断累加即可，相当于将n次乘法和加法分开计算
                for (std::size_t k = start; k < end; k++)
                    tmp[rhs.m_JA[k]] += m_AA[j] * rhs.m_AA[k];
            }

            // 去掉过小的元素
            for (std::size_t j = 0; j < col2; j++)
                if (std::abs(tmp[j]) > std::numeric_limits<Real>::epsilon())
                    res(i, j) = tmp[j];
        }
        return res;
    }

    /**
     * @brief 取反
     *
     * @return spmatx
     */
    spmatx operator-() const
    {
        spmatx res(m_row, m_col);
        res.m_IA = m_IA;
        res.m_JA = m_JA;
        res.m_AA.resize(m_AA.size());
        for (std::size_t i = 0; i < m_AA.size() - 1; i++)
            res.m_AA[i] = -m_AA[i];
        res.m_AA.back() = m_AA.back();
        return res;
    }

    /**
     * @brief 范数
     *
     * @param[in] nt
     * @return Real
     */
    Real norm(int nt = 0) const
    {
        switch (nt)
        {
        case 0: // inf-norm
        {
            Real r = 0;
            for (std::size_t i = 0; i < m_row; i++)
            {
                Real a = 0;
                // IA[m] = nnz，边界值
                for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                    a += std::abs(m_AA[j]);
                r = std::max(a, r);
            }
            return r;
        }
        case 2: // f-norm
            // 注意这里 Real{} 将决定返回值类型，如果直接设为 0 会按照 int 类型返回，导致结果错误
            return std::inner_product(m_AA.begin(), m_AA.end() - 1, m_AA.begin(), Real{});
        default:
            throw std::logic_error("Invalid norm type");
            return 0;
        }
    }

    /**
     * @brief 迹
     *
     * @return Real
     */
    Real trace() const
    {
        Real tr = 0;
        std::size_t n = std::min(m_row, m_col);
        for (std::size_t i = 0; i < n; ++i)
            tr += operator()(i, i);
        return tr;
    }

    /**
     * @brief 转置矩阵
     *
     * @return spmatx&
     */
    spmatx &transpose()
    {
        spmatx res(m_col, m_row);
        for (std::size_t i = 0; i < m_row; i++)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                res(m_JA[j], i) = m_AA[j];
        }
        m_IA = std::move(res.m_IA);
        m_JA = std::move(res.m_JA);
        m_AA = std::move(res.m_AA);
        std::swap(m_row, m_col);
        return *this;
    }

    /**
     * @brief 转置矩阵
     *
     * @return spmatx
     */
    spmatx transposed() const
    {
        spmatx res(m_col, m_row);
        for (std::size_t i = 0; i < m_row; i++)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                res(m_JA[j], i) = m_AA[j];
        }
        return res;
    }

    /**
     * @brief 压缩矩阵
     *
     * @return spmatx&
     */
    spmatx &compress()
    {
        spmatx res(m_row, m_col);
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                if (std::abs(m_AA[j]) > std::numeric_limits<Real>::epsilon())
                    res(i, m_JA[j]) = m_AA[j];
        }
        *this = std::move(res);
        return *this;
    }

    /**
     * @brief 压缩矩阵
     *
     * @return spmatx
     */
    spmatx compressed() const
    {
        spmatx res(m_row, m_col);
        for (std::size_t i = 0; i < m_row; ++i)
        {
            // IA[m] = nnz，边界值
            for (std::size_t j = m_IA[i]; j < m_IA[i + 1]; j++)
                if (std::abs(m_AA[j]) > std::numeric_limits<Real>::epsilon())
                    res(i, m_JA[j]) = m_AA[j];
        }
        return res;
    }

    /**
     * @brief 单位阵
     *
     * @param[in] n
     * @return spmatx
     */
    static spmatx identity(std::size_t n)
    {
        spmatx res(n, n);
        for (std::size_t i = 0; i < n; ++i)
            res(i, i) = 1;
        return res;
    }
};

template <std::size_t M, typename Real> struct diagmatx : public matx<Real>
{
  private:
    static_assert(M % 2 == 1, "Invalid matrix size");
    static constexpr std::size_t _mid = M / 2; // 标记中线位置

  public:
    using matx<Real>::m_data;
    using matx<Real>::rows;
    using matx<Real>::cols;

    diagmatx(const diagmatx &rhs) = default;
    diagmatx(diagmatx &&rhs) = default;

    // 移除不需要的函数
    void resize(std::size_t row, std::size_t col, bool keep = false) = delete;
    matx<Real> operator%(const matx<Real> &rhs) const = delete;
    matx<Real> &fill(std::size_t i, std::size_t j, const matx<Real> &rhs) = delete;
    matx<Real> &invert() = delete;
    matx<Real> inverted() const = delete;

    /**
     * @brief 矩阵规模 M x n
     * 0
     * 1 1
     * 1 1 1
     *   1 1 1
     *     1 1
     *       0
     *
     * @param[in] n
     */
    explicit diagmatx(std::size_t n) : matx<Real>(M, n)
    {
    }

    /**
     * @brief 拷贝构造
     *
     * @param[in] rhs
     */
    diagmatx(const matx<Real> &rhs) : matx<Real>(rhs)
    {
        if (rhs.rows() != M || rhs.cols() != cols())
            throw std::logic_error("Invalid matrix size");
    }

    /**
     * @brief 移动构造
     *
     * @param[in] rhs
     */
    diagmatx(matx<Real> &&rhs) : matx<Real>(std::move(rhs))
    {
        if (rhs.rows() != M || rhs.cols() != cols())
            throw std::logic_error("Invalid matrix size");
    }

    /**
     * @brief 拷贝赋值
     *
     * @param[in] rhs
     * @return diagmatx&
     */
    diagmatx &operator=(const diagmatx &rhs)
    {
        if (this != &rhs) [[likely]]
            m_data = rhs.m_data;
        return *this;
    }

    /**
     * @brief 移动赋值
     *
     * @param[in] rhs
     * @return diagmatx&
     */
    diagmatx &operator=(diagmatx &&rhs)
    {
        if (this != &rhs) [[likely]]
            m_data = std::move(rhs.m_data);
        return *this;
    }

    /**
     * @brief 转换为矩阵（不使用隐式，因为 diagmatx 作为子类会直接匹配到 matx 的拷贝构造函数）
     *
     * @return matx
     */
    matx<Real> to_matrix() const
    {
        matx<Real> res(cols(), cols());
        res.set_zero();

        for (int i = 0; i < cols(); ++i)
        {
            int n = std::min(i + int(_mid) + 1, int(cols()));
            for (int j = std::max(i - int(_mid), 0); j < n; ++j)
                res(i, j) = operator()(i, j);
        }
        return res;
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return Real
     */
    Real operator()(std::size_t i, std::size_t j) const
    {
        if (i >= cols() || j >= cols() || std::abs(int(i) - int(j)) >= _mid + 1)
            throw std::out_of_range("Index out of range");
        return m_data[(_mid + i - j) * cols() + j];
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return Real&
     */
    Real &operator()(std::size_t i, std::size_t j)
    {
        if (i >= cols() || j >= cols() || std::abs(int(i) - int(j)) >= _mid + 1)
            throw std::out_of_range("Index out of range");
        return m_data[(_mid + i - j) * cols() + j];
    }

    /**
     * @brief 由于重写了矩阵乘法，需要重写相关函数
     *
     * @param[in] t
     * @return diagmatx
     */
    diagmatx operator*(Real t) const
    {
        diagmatx res(cols());
        std::transform(m_data.begin(), m_data.end(), res.m_data.begin(), [t](Real x) { return x * t; });
        return res;
    }

    /**
     * @brief 由于重写了矩阵乘法，需要重写相关函数
     *
     * @param[in] t
     * @return diagmatx&
     */
    diagmatx &operator*=(Real t)
    {
        std::transform(m_data.begin(), m_data.end(), m_data.begin(), [t](Real x) { return x * t; });
        return *this;
    }

    /**
     * @brief 矩阵乘法
     *
     * @param[in] rhs
     * @return diagmatx<M + _mid * 2, Real>
     */
    diagmatx<M + _mid * 2, Real> operator*(const diagmatx &rhs) const
    {
        if (cols() != rhs.cols())
            throw std::logic_error("matx size not match");

        diagmatx<M + _mid * 2, Real> res(cols());
        res.set_zero();

        for (int i = 0; i < cols(); ++i)
        {
            int n1 = std::min(i + int(_mid * 2) + 1, int(cols()));
            for (int j = std::max(i - int(_mid * 2), 0); j < n1; ++j)
            {
                int n2 = std::min(i + int(_mid) + 1, std::min(j + int(_mid) + 1, int(cols())));
                for (int k = std::max(i - int(_mid), std::max(j - int(_mid), 0)); k < n2; ++k)
                    res(i, j) += operator()(i, k) * rhs(k, j);
            }
        }
        return res;
    }

    /**
     * @brief 范数
     *
     * @param[in] nt
     * @return Real
     */
    Real norm(int nt = 0) const
    {
        switch (nt)
        {
        case 0: // inf-norm
        {
            Real r = 0;
            for (int i = 0; i < cols(); i++)
            {
                Real a = 0;
                int n = std::min(i + int(_mid) + 1, int(cols()));
                for (int j = std::max(i - int(_mid), 0); j < n; j++)
                    a += std::abs(operator()(i, j));
                r = std::max(a, r);
            }
            return r;
        }
        case 1: // 1-norm
        {
            Real r = 0;
            for (int i = 0; i < cols(); i++)
            {
                Real a = 0;
                int n = std::min(i + int(_mid) + 1, int(cols()));
                for (int j = std::max(i - int(_mid), 0); j < n; j++)
                    a += std::abs(operator()(j, i));
                r = std::max(a, r);
            }
            return r;
        }
        case 2: // f-norm
        {
            // 注意这里 Real{} 将决定返回值类型，如果直接设为 0 会按照 int 类型返回，导致结果错误
            Real r = 0;
            for (int i = 0; i < cols(); i++)
            {
                int n = std::min(i + int(_mid) + 1, int(cols()));
                for (int j = std::max(i - int(_mid), 0); j < n; j++)
                    r += std::pow(operator()(i, j), 2);
            }
            return r;
        }
        default:
            throw std::logic_error("Invalid norm type");
            return 0;
        }
    }

    /**
     * @brief 迹
     *
     * @return Real
     */
    Real trace() const
    {
        // 累计对角元素之和，注意 Real{} 将决定返回值类型，如果直接设为 0 会按照 int 类型返回，导致结果错误
        return std::accumulate(m_data.begin() + _mid * cols(), m_data.begin() + (_mid + 1) * cols(), Real{});
    }

    /**
     * @brief 转置矩阵
     *
     * @return diagmatx&
     */
    diagmatx &transpose()
    {
        for (int i = 0; i < cols(); ++i)
            for (int j = std::max(i - int(_mid), 0); j < i + 1; ++j)
                std::swap(operator()(i, j), operator()(j, i));
        return *this;
    }

    /**
     * @brief 转置
     *
     * @return diagmatx
     */
    diagmatx transposed() const
    {
        diagmatx res(cols());
        for (int i = 0; i < cols(); ++i)
        {
            int n = std::min(i + int(_mid) + 1, int(cols()));
            for (int j = std::max(i - int(_mid), 0); j < n; ++j)
                res(i, j) = operator()(j, i);
        }
        return res;
    }

    /**
     * @brief 单位阵
     *
     * @return diagmatx
     */
    static diagmatx identity(std::size_t n)
    {
        diagmatx res(n);
        res.set_zero();
        for (std::size_t i = 0; i < n; ++i)
            res(i, i) = 1;
        return res;
    }

    /**
     * @brief 从矩阵构建
     *
     * @param[in] rhs
     * @return diagmatx
     */
    static diagmatx from_matrix(const matx<Real> &rhs)
    {
        diagmatx res(rhs.cols());
        for (int i = 0; i < res.m_col; ++i)
        {
            int n = std::min(i + int(_mid) + 1, int(res.m_col));
            for (int j = std::max(i - int(_mid), 0); j < n; ++j)
                res(i, j) = rhs(i, j);
        }
        return res;
    }
};

using matxd = matx<double>;
using spmatxd = spmatx<double>;
using vecxd = vecx<double>;
using spvecxd = spvecx<double>;
using diagmat1d = diagmatx<1, double>;
using diagmat3d = diagmatx<3, double>;
using diagmat5d = diagmatx<5, double>;

/**
 * @brief 流式输出
 *
 * @tparam Real
 * @param os
 * @param m
 * @return std::ostream&
 */
template <typename Real> inline std::ostream &operator<<(std::ostream &os, const matx<Real> &m)
{
    os << "{" << std::endl;
    for (std::size_t i = 0; i < m.rows(); i++)
    {
        os << "{";
        for (std::size_t j = 0; j < m.cols(); j++)
            os << "\t" << m(i, j);
        os << "\t}" << std::endl;
    }
    os << "}";
    return os;
}

/**
 * @brief 流式输出
 *
 * @tparam Real
 * @param os
 * @param m
 * @return std::ostream&
 */
template <typename Real> inline std::ostream &operator<<(std::ostream &os, const spmatx<Real> &m)
{
    os << "{" << std::endl;
    for (std::size_t i = 0; i < m.rows(); i++)
        for (std::size_t k = m.m_IA[i]; k < m.m_IA[i + 1]; k++)
            os << "(" << i << ", " << m.m_JA[k] << ") -> " << m.m_AA[k] << std::endl;
    os << "}";
    return os;
}

/**
 * @brief 向量内积
 *
 * @param[in] lhs
 * @param[in] rhs
 * @return double
 */
template <typename Real> inline double dot(const matx<Real> &lhs, const matx<Real> &rhs)
{
    if (lhs.rows() != rhs.rows() || lhs.cols() != rhs.cols())
        throw std::logic_error("matx size not match");

    // 注意这里 Real{} 将决定返回值类型，如果直接设为 0 会按照 int 类型返回，导致结果错误
    return std::inner_product(lhs.data(), lhs.data() + lhs.rows() * lhs.cols(), rhs.data(), Real{});
}

/**
 * @brief 求逆
 *
 * @tparam Real
 * @param[in] m
 * @return matx<Real>
 */
template <typename Real> inline matx<Real> invert(const matx<Real> &A_)
{
    if (A_.rows() != A_.cols())
        throw std::logic_error("matx is not square");

    // 初始化单位阵
    matx A = A_;
    matx m = matx<Real>::identity(A.rows());

    Real max;          // 最大值
    std::size_t index; // 最大值位置

    for (std::size_t i = 0; i < A.cols(); i++)
    {
        max = Real{};
        index = i;

        // 寻找最大列元作为主元
        for (std::size_t j = i; j < A.rows(); j++)
        {
            Real tmp = std::abs(A(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }

        if (max < std::numeric_limits<Real>::epsilon())
            throw std::runtime_error("matx not invertible");

        // 交换主元
        for (std::size_t q = 0; q < A.cols(); q++)
        {
            std::swap(A(index, q), A(i, q));
            std::swap(m(index, q), m(i, q));
        }

        // Gauss 消去
        for (std::size_t j = 0; j < A.rows(); j++)
        {
            Real tmp = A(i, i);
            if (i == j)
            {
                for (std::size_t k = 0; k < A.cols(); k++)
                {
                    m(j, k) /= tmp;
                    A(j, k) /= tmp;
                }
                continue;
            }
            tmp = A(j, i) / A(i, i);
            for (std::size_t k = 0; k < A.cols(); k++)
            {
                m(j, k) -= tmp * m(i, k);
                A(j, k) -= tmp * A(i, k);
            }
        }
    }
    return m;
}

/**
 * @brief 计算矩阵的秩，返回符号表示行列式的正负
 *
 * @tparam Real
 * @param[in] A
 * @return int
 */
template <typename Real> inline int rank(const matx<Real> &A_)
{
    matx A = A_;

    Real max;    // 最大值
    int index;   // 最大值位置
    int inv = 0; // 交换次数
    int r = 0;
    for (int i = 0; i < A.cols(); i++)
    {
        max = Real{};
        index = i;

        // 寻找最大列元作为主元
        for (std::size_t j = i; j < A.rows(); j++)
        {
            Real tmp = std::abs(A(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }

        // 跳过空的列
        if (max <= std::numeric_limits<Real>::epsilon())
            continue;

        // 交换主元
        for (std::size_t q = 0; q < A.cols(); q++)
            std::swap(A(index, q), A(i, q));

        // 记录交换次数（交换任何两行都会改变行列式符号）
        inv += i == index ? 0 : 1;

        // Gauss 消去
        for (std::size_t j = i + 1; j < A.rows(); j++)
        {
            A(j, i) /= A(i, i);
            for (std::size_t k = i + 1; k < A.cols(); k++)
                A(j, k) -= A(i, k) * A(j, i);
        }

        r++;
    }
    return r * (inv % 2 == 0 ? 1 : -1);
}

/**
 * @brief 计算矩阵的最大模特征值的绝对值
 *
 * @tparam Real
 * @param[in] A
 * @return Real
 */
template <typename Real> inline Real eigenvalue(const matx<Real> &A)
{
    if (A.rows() != A.cols())
        throw std::logic_error("matx is not square");

    auto n = A.rows();
    matx<Real> x(n, 1);
    matx<Real> y(n, 1);
    x.set_zero();
    x(0, 0) = 1;

    Real dp = 1;
    Real q = 0;

    // 精度要求不能太高，不然算不出来
    std::size_t count = 0;
    while (dp > numeric::half && count++ < numeric::iter)
    {
        y = A * x;
        Real p = y.norm();
        x = y / p;
        dp = std::abs(p - q);
        q = p;
    }

    return q;
}

/**
 * @brief 矩阵左除，注意右逆乘在左边
 *
 * @tparam Real
 * @param[in] lhs
 * @param[in] rhs
 * @return matx<Real>
 */
template <typename Real> inline matx<Real> operator/(const matx<Real> &lhs, const matx<Real> &rhs)
{
    return invert(rhs) * lhs;
}

/**
 * @brief 共轭梯度法求解 Ax = b
 *
 * @tparam Real
 * @param[in] A
 * @param[in] b
 * @return vecx<Real>
 */
template <typename Real> inline vecx<Real> conjugate_gradient(const matx<Real> &A, const vecx<Real> &b)
{
    std::size_t n = A.cols();
    std::size_t count = 0;

    vecx<Real> x(n);
    x.set_zero();
    auto p = b - A * x;
    auto r = p;

    Real rr = dot(r, r); // r0 * r0, next r0 * r0
    while (std::sqrt(rr) > numeric::half && count++ < numeric::iter)
    {
        Real alpha = rr / dot(p, (A * p)); // alpha_0 = r0T * r0 / p0T * A * p0
        x = x + p * alpha;                 // xk = xk-1 + alphak-1 * pk-1
        r = r - A * p * alpha;             // r = rk-1 - alphak-1 * A * pk-1
        Real nrr = dot(r, r);              // next r * r, nrr = r * r
        Real beta = nrr / rr;              // beta = rkT * rk / rk-1T * rk-1
        rr = nrr;                          // p = rk + betak-1 * pk-1
        p = r + p * beta;                  // alpha
    }
    return x;
}

/**
 * @brief 二分法，计算三对角对称阵的全部特征值
 *
 * @tparam Real
 * @param[in] A
 * @return vecx<Real>
 */
template <typename Real> inline vecx<Real> sym_eigenvalues(const diagmatx<3, Real> &A)
{
    // 特征值范围由无穷范数控制
    std::size_t n = A.cols();
    Real u = A.norm(), l = -u;

    // 初始化系数
    vecx<Real> lambda(n), alpha(n), beta(n);
    for (std::size_t i = 0; i < n; i++)
    {
        alpha[i] = A(i, i);
        if (i > 0)
            beta[i] = A(i, i - 1);
    }

    // 计算 n 个特征值
    for (std::size_t i = 0; i < n; i++)
    {
        Real _u = u;
        Real _l = l;
        Real nowE = u - l;
        Real r = (u + l) / 2;
        while (nowE > numeric::half)
        {
            // 计算 sn(r)，即 q1(r),q2(r),...,qn(r) 中负数的个数
            std::size_t s = 0;
            Real q = alpha[0] - r;
            for (std::size_t j = 0; j < n; j++)
            {
                if (q < 0)
                    s++;

                if (j < n - 1)
                {
                    // replace q with a little positive number
                    if (std::abs(q) < numeric::half)
                        q = std::abs(beta[j + 1]) * numeric::half;
                    q = alpha[j + 1] - r - beta[j + 1] * beta[j + 1] / q;
                }
            }
            // 与 i+1 进行比较
            (s >= (i + 1)) ? _u = r : _l = r;
            r = (_u + _l) / 2;
            nowE = _u - _l;
        }
        lambda[i] = r;
    }
    return lambda;
}

} // namespace math

} // namespace xy
